#include<ros/ros.h>
#include <iostream>
#include <fstream>
#include <sstream>

#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>


#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/CameraInfo.h>
#include <gazebo_msgs/LinkStates.h>
#include <geometry_msgs/Twist.h>

using namespace std;
using namespace cv;



int current_time = 0;
float camera_height = 60.3;

//define the inverse of intrinsic matrix for all cameras(assign value in main)
Mat K = Mat::zeros(3,3,CV_32FC1);
Mat K_ivt;
bool flag_K = false;


//int border_x = 0;
//int border_y = 0;

//IPM for one frame of one camera, and rotation_z is the angle of counterclockwise rotation of camera.
void partIPM(Mat& IPM,const Mat& color,float rotation_z)
{
    for(int v = 0 ; v < color.rows; v++)
    {
        for(int u = 0; u < color.cols; u++)
        {


                // convert the pixel_coordinate to camera to camera_trans to base to IPM_pixel_coordinate
                Mat P_pixel = Mat::ones(3,1,CV_32FC1);
                P_pixel.at<float>(0,0) = u;
                P_pixel.at<float>(1,0) = v;
                Mat P_camera = K_ivt*P_pixel*50;

                if(P_camera.at<float>(1,0) > 10)
                {
                    float proportion = 60.3/P_camera.at<float>(1,0);
                    //float proportion = 1;
                    Mat P_cameraTrans = Mat::ones(3,1,CV_32FC1);
                    P_cameraTrans.at<float>(0,0) = proportion * P_camera.at<float>(2,0);
                    P_cameraTrans.at<float>(1,0) = (-1 * proportion) * P_camera.at<float>(0,0);

                    Mat P_base = Mat::ones(3,1,CV_32FC1);
                    P_base.at<float>(0,0) = P_cameraTrans.at<float>(0,0)*cos(rotation_z) + P_cameraTrans.at<float>(1,0)*sin(rotation_z);
                    P_base.at<float>(1,0) = P_cameraTrans.at<float>(1,0)*cos(rotation_z) - P_cameraTrans.at<float>(0,0)*sin(rotation_z);

                    //if(P_base.at<float>(0,0) > border_x)border_x = P_base.at<float>(0,0);
                    //if(P_base.at<float>(1,0) > border_y)border_y = P_base.at<float>(1,0);
                    int IPM_u = 1300 - P_base.at<float>(1,0);
                    int IPM_v = 1300 - P_base.at<float>(0,0);
                    IPM.at<Vec3b>(IPM_v,IPM_u)[0] = color.at<Vec3b>(v,u)[2];
                    IPM.at<Vec3b>(IPM_v,IPM_u)[1] = color.at<Vec3b>(v,u)[1];
                    IPM.at<Vec3b>(IPM_v,IPM_u)[2] = color.at<Vec3b>(v,u)[0];
                }

                //cout << u << ' ' << v << endl << P_base.at<float>(0,0)  << endl;
                //cout << border_x << endl << border_y << endl;


        }
    }
}

//get the intrinsic param and assign value to global K and set the bool of K to true.
void cmrInfoCallback(const sensor_msgs::CameraInfoConstPtr& cmr_info)
{
     K.at<float>(0,0) = cmr_info->K[0];
     K.at<float>(0,2) = cmr_info->K[2];
     K.at<float>(1,1) = cmr_info->K[4];
     K.at<float>(1,2) = cmr_info->K[5];
     K.at<float>(2,2) = cmr_info->K[8];
    invert(K,K_ivt);
    flag_K = true;
}


// if flag_K, do IPM.
void cmrCallback(const sensor_msgs::ImageConstPtr& img0,
                 const sensor_msgs::ImageConstPtr& img1,
                 const sensor_msgs::ImageConstPtr& img2,
                 const sensor_msgs::ImageConstPtr& img3)
{
    if(flag_K)
    {
        ostringstream buffer;

        Mat IPM = Mat::zeros(2600,2600,CV_8UC3);//store the IPM image.


        Mat colorMat0 = cv_bridge::toCvShare(img0)->image;
        Mat colorMat1 = cv_bridge::toCvShare(img1)->image;
        Mat colorMat2 = cv_bridge::toCvShare(img2)->image;
        Mat colorMat3 = cv_bridge::toCvShare(img3)->image;

        partIPM(IPM,colorMat0,0);
        partIPM(IPM,colorMat1,-1.5707963267949);
        partIPM(IPM,colorMat2,3.141592653589793);
        partIPM(IPM,colorMat3,1.5707963267949);
        //imwrite("/home/nnlxd/dataset/00/image/ipm_img/05.jpg",depthMat0);

        buffer << "/home/nnlxd/dataset/00/image/ipm_img1/" << img0->header.stamp << ".jpg";
        string str = buffer.str();
        cv::imwrite(str,IPM);
        cout << "write one" << endl;
    }
    else
        cout << "No camera parameters!" << endl;
}
int main(int argc, char** argv)
{
    ros::init(argc,argv,"recorder");
    ros::NodeHandle nh;
    ros::NodeHandle n;

    //rewrite it to time-synchronize filter form.

    message_filters::Subscriber<sensor_msgs::Image> cmr0(nh,"/robot_1/camera0/color/image_raw",1);
    message_filters::Subscriber<sensor_msgs::Image> cmr1(nh,"/robot_1/camera1/color/image_raw",1);
    message_filters::Subscriber<sensor_msgs::Image> cmr2(nh,"/robot_1/camera2/color/image_raw",1);
    message_filters::Subscriber<sensor_msgs::Image> cmr3(nh,"/robot_1/camera3/color/image_raw",1);


    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image>
    sync(cmr0,cmr1, cmr2, cmr3, 10);

    sync.registerCallback(boost::bind(&cmrCallback, _1, _2, _3, _4));

    ros::Subscriber camera_info = n.subscribe<sensor_msgs::CameraInfo>("/robot_1/camera0/color/camera_info",1000,cmrInfoCallback);

    ros::spin();

}